#include "Timer.h"
#include "message.h"
#include "../pr.h"
#include "../hotmacCtp.h"

#define INITIAL_INTERVAL 400*1024L
// #define SEND_INTERVAL 60 * 1024L
#define SEND_INTERVAL 4 * 1024L
#define SNIFFER_INTERVAL 20*1024L
#define NODE_SIZE 14

module hotmacCtpP {
  uses {
    // Control
    interface Boot;
    interface SplitControl as SerialControl;
    interface SplitControl as RadioControl;
    interface AsyncSplitControl as EnergyControl;
    interface StdControl as RoutingControl;
    // Timer
    interface Timer<TMilli> as TxTimer;
    interface Timer<TMilli> as SnifferTimer;
    interface LocalTime<TMilli> as LocalTime;
    // CTP
    interface RootControl;
    interface CtpInfo;
    interface Intercept;
    interface Send;
    interface Receive;
    // Misc
    interface TimeSyncAMSend<TMilli, uint32_t> as SnifferSend;
    interface Packet as RadioPacket;
    interface HotmacInfo as SendInfo;
    interface HotmacInfo as ReceiveInfo;
    interface LinkEstimator;
    interface CC2420Packet;
    interface LowPowerListening;
    interface Random;
    interface Leds;
    // UART
    interface Queue<message_t*>;
    interface Pool<message_t>;
    interface AMSend as UARTSend;
  }
}
implementation {
  message_t Pkt;
  message_t snifferPkt;

  uint16_t SeqNo = 0;
  uint16_t snifferSeqNo = 0;
  uint16_t forwardcount = 0;
  uint32_t livetime = 0;
  uint32_t dutytime = 0;
  uint32_t temptime = 0;
  bool Busy = FALSE;
  bool uartBusy = FALSE;

  void SendMsg();
  void ClearMsg();
  task void uartEchoTask();

  event void Boot.booted() {
    call SerialControl.start();
    Busy = FALSE;
    uartBusy = FALSE;
  }

  event void SerialControl.startDone(error_t err) {
    if (err != SUCCESS) {
      call SerialControl.start();
    } else {
      call RadioControl.start();
    }
  }

  event void RadioControl.startDone(error_t err) {
    if (err != SUCCESS) {
      call RadioControl.start();
    } else {
      call RoutingControl.start();
      call Leds.led0On();
      if (TOS_NODE_ID == ROOT_ID) {
        pr("This is Sink!\n");
        call RootControl.setRoot();
      } else {
        pr("This is node%d!\n", TOS_NODE_ID);
        call TxTimer.startOneShot(INITIAL_INTERVAL);
        livetime = call LocalTime.get();
      }
    }
  }

  event void RadioControl.stopDone(error_t err) { }
  event void SerialControl.stopDone(error_t err) { }

  async event void EnergyControl.startDone(error_t err) {
    if (err == SUCCESS) {
      temptime = call LocalTime.get();
    }
  }

  event void EnergyControl.stopDone(error_t err) {
    if (err == SUCCESS) {
      atomic dutytime += call LocalTime.get() - temptime;
    }
  }

  event void Send.sendDone(message_t* msg, error_t err) {
    if (err != SUCCESS) {
      pr("The transmission of pkt with sequence number %d is failed!\n", SeqNo);
    } else {
      pr("The transmission of pkt with sequence number %d is success\n", SeqNo);
    }
    call TxTimer.startOneShot(SEND_INTERVAL/2 + (call Random.rand32()%(SEND_INTERVAL/2)));
    ClearMsg();
  }

  event void UARTSend.sendDone(message_t* msg, error_t error) {
    uartBusy = FALSE;
    call Pool.put(msg);
    if (!call Queue.empty()) {
      post uartEchoTask();
    }
  }

  event bool Intercept.forward(message_t* msg, void* payload, uint8_t len) {
    forwardcount++;
    call Leds.led2Toggle();
    return TRUE;
  }

  event message_t* Receive.receive(message_t* msg, void* payload, uint8_t len) {
    call Leds.led2Toggle();
    pr("Receive a packet!\n");

    if (!call Pool.empty() && call Queue.size() < call Queue.maxSize()) {
      message_t* tmp = call Pool.get();
      if (tmp == NULL) {
        pr("The pointer from Pool is NULL!\n");
        return msg;
      }
      call Queue.enqueue(msg);
      if (!uartBusy) {
        post uartEchoTask();
      }
      return tmp;
    }

    return msg;
  }

  event void TxTimer.fired() {
    if (!Busy) {
      pr("Send a new packet\n");
      call Leds.led0Toggle();
      SendMsg();
    } else {
      pr("Sending is busy! Try again later!\n");
      SeqNo++;
      call TxTimer.startOneShot(SEND_INTERVAL/2 + call Random.rand32()%(SEND_INTERVAL/2));
    }
  }

  void SendMsg() {
    am_addr_t Parent = 0;
    uint16_t tmpEtx = 0;
    hotmac_msg_t* msg = (hotmac_msg_t*)call Send.getPayload(&Pkt, sizeof(hotmac_msg_t));
    uint8_t maxLen = call Send.maxPayloadLength();

    if (msg == NULL) {
      pr("msg is a NULL pointer!\n");
      return;
    }

    if (call CtpInfo.getParent(&Parent) != SUCCESS) {
      pr("There's no nexthop forwarder!\n");
      call TxTimer.startOneShot(INITIAL_INTERVAL);
      return;
    }

    if (sizeof(hotmac_msg_t) > maxLen) {
      pr("Length is %d which overwhelm maxLen %d!\n", sizeof(hotmac_msg_t), maxLen);
      return;
    }

    SeqNo++;
    call CtpInfo.getEtx(&tmpEtx);

    msg->id = TOS_NODE_ID;
    msg->seqno = SeqNo;
    msg->parentId = Parent;
    msg->etx = tmpEtx;
    msg->forwardcont = forwardcount;
    msg->livetime = call LocalTime.get() - livetime;
    msg->dutytime = dutytime;

    // call LplInfo.getLplInfo(msg);
    call SendInfo.getSendInfo(msg);
    call ReceiveInfo.getReceiveInfo(msg);

    if (call Send.send(&Pkt, sizeof(hotmac_msg_t)) != SUCCESS) {
      pr("Ctp doesn't accept the packet!\n");
      call TxTimer.startOneShot(call Random.rand32()%(SEND_INTERVAL/2));
      return;
    } else {
      pr("Ctp accepts the packet successfully!\n");
      Busy = TRUE;
    }
  }

  event void LinkEstimator.evicted(am_addr_t neighbor) {
  }

  void ClearMsg() {
    Busy = FALSE;
  }

  event void SnifferSend.sendDone(message_t* msg, error_t err) {
    ClearMsg();
    call SnifferTimer.startOneShot(SNIFFER_INTERVAL);
  }

  event void SnifferTimer.fired() {
    uint32_t time = call LocalTime.get();
    // uint32_t tmptime;
    uint16_t tmpEtx;
    am_addr_t Parent;
    uint8_t i = 0;
    sniffer_msg_t* msg = (sniffer_msg_t*)call SnifferSend.getPayload(&snifferPkt, sizeof(sniffer_msg_t));

    if (Busy) {
      call SnifferTimer.startOneShot(SNIFFER_INTERVAL);
      return;
    }

    call CtpInfo.getEtx(&tmpEtx);
    call CtpInfo.getParent(&Parent);

    msg->id = TOS_NODE_ID;
    msg->seqno = snifferSeqNo;
    msg->parentId = Parent;
    msg->etx = tmpEtx;

    snifferSeqNo++;

    msg->size = call CtpInfo.numNeighbors();
    for (i=0;i<msg->size;i++) {
      msg->lnk[i].id = call CtpInfo.getNeighborAddr(i);
      msg->lnk[i].etx = call LinkEstimator.getLinkQuality(msg->lnk[i].id);
    }

    // call ProbeInfo.getProbeTime(&tmptime); // schedule info
    // msg->interval = (tmptime - time) % LPL_DEF_LOCAL_WAKEUP;

    call CC2420Packet.setPower(&snifferPkt, 31);

    if ( call SnifferSend.send(SPECIAL_ID, &snifferPkt, getPacketLength(msg), time) != SUCCESS) {
      call SnifferTimer.startOneShot(SNIFFER_INTERVAL);
    } else {
      Busy = TRUE;
    }
  }

  task void uartEchoTask() {
    if (call Queue.empty()) {
      return;
    } else if (!uartBusy) {
      message_t* tmp = call Queue.dequeue();
      uint8_t tmpLen = call RadioPacket.payloadLength(tmp);
      if (call UARTSend.send(0xffff, tmp, tmpLen) == SUCCESS) {
        uartBusy = TRUE;
      } else if (!call Queue.empty()){
        post uartEchoTask();
      }
    }
  }
}
